#include "drivers.hpp"
Drivers::Drivers() {
    node_handle_ptr_ = std::make_shared<ros::NodeHandle>("");
    // imu_ptr_ = std::make_shared<Imu>(node_handle_ptr_, kImuPort);
    // imu_thread_ptr_ = std::make_shared<std::thread>(&Imu::ImuRxThread,
    // imu_ptr_);
    // lidar_ptr_ = std::make_shared<Lidar>(node_handle_ptr_, kLidarPort);
    // lidar_thread_ptr_ =
    //     std::make_shared<std::thread>(&Lidar::LidarRxThread, lidar_ptr_);
    // for (auto&& serial_port : kUltrasonicPorts) {
    //   std::shared_ptr<Ultrasonic> ultrasonic_ptr =
    //       std::make_shared<Ultrasonic>(node_handle_ptr_, serial_port);
    //   std::shared_ptr<std::thread> ultrasonic_thread_ptr =
    //       std::make_shared<std::thread>(&Ultrasonic::UltrasonicRxThread,
    //                                     ultrasonic_ptr);
    //   ultrasonic_ptrs_.push_back(ultrasonic_ptr);
    //   ultrasonic_thread_ptrs_.push_back(ultrasonic_thread_ptr);
    // }

    // for (auto&& serial_port : kLinelaserPorts) {
    //   std::shared_ptr<Linelaser> linelaser_ptr =
    //       std::make_shared<Linelaser>(node_handle_ptr_, lserial_port);
    //   std::shared_ptr<std::thread> linelaser_thread_ptr =
    //       std::make_shared<std::thread>(&Linelaser::LinelaserRxThread,
    //                                     linelaser_ptr);
    //   linelaser_ptrs_.push_back(linelaser_ptr);
    //   linelaser_thread_ptrs_.push_back(linelaser_thread_ptr);
    // }

    for (auto &&bus_port : kCameraPorts) {
        std::shared_ptr<Camera> camera_ptr =
            std::make_shared<Camera>(node_handle_ptr_, bus_port);
        std::shared_ptr<std::thread> camera_thread_ptr =
            std::make_shared<std::thread>(&Camera::CameraRxThread, camera_ptr);
        camera_ptrs_.push_back(camera_ptr);
        camera_thread_ptrs_.push_back(camera_thread_ptr);
    }
};
Drivers::~Drivers() {
    for (auto &&linelaser_ptr : linelaser_ptrs_)
        linelaser_ptr->SetRunning(false);
    for (auto &&linelaser_thread_ptr : linelaser_thread_ptrs_) {
        if ((linelaser_thread_ptr != nullptr) &&
            linelaser_thread_ptr->joinable()) {
            linelaser_thread_ptr->join();
            linelaser_thread_ptr.reset();
        }
    }
    for (auto &&ultrasonic_ptr : ultrasonic_ptrs_)
        ultrasonic_ptr->SetRunning(false);
    for (auto &&ultrasonic_thread_ptr : ultrasonic_thread_ptrs_) {
        if ((ultrasonic_thread_ptr != nullptr) &&
            ultrasonic_thread_ptr->joinable()) {
            ultrasonic_thread_ptr->join();
            ultrasonic_thread_ptr.reset();
        }
    }
};
